package solver;

import java.util.ArrayList;
import java.util.Arrays;

import function.ConflictFunction;

public class GradientDescentSolver {
	
	private static double threshold = 1E-6;
	private static int counter_max = 100000;



    public static double[][] solve(ConflictFunction func) {
            System.out.println("Solver started.");
            int L = func.getDimension();
            double[] bounds = func.getBounds();            
            double[][] x_old = new double[L][L];
            double[][] x_new = initializeX(bounds);
            double[][] grad;
            int iteration=1;
            double h = 0;
            double error = 1E99;
            
            while ((error > threshold) && (iteration < counter_max)) {
            	if (iteration%1000 == 0) 
                System.out.println("Iteration: " + iteration + "  Error:" + error);
                h = 1 / Math.sqrt(iteration);
                x_old = Utility.copyMatrix(x_new);
                grad = getDerivatives(func, x_new);
                grad = Utility.multMatrixConstant(grad,h);
                x_new = Utility.addMatrix(x_new,grad);
                   
                for (int i=0; i<L; i++) {
                   x_new[i] = project(x_new[i],bounds[i]);
                }
                    
                iteration++;
                error = error(x_new,x_old,h);
            }
            System.out.println("Iteration: " + iteration + "  Error:" + error);
            
            return x_new;
    }

	public static double[] project2(double[] theta, double pi) {
		int L = theta.length;
		if (theta.length == 0) System.out.println("Theta length is zero!");
//		System.out.println("Pi:"+ pi);
//		Utility.printArray(theta);
//		sorted_theta = sort(theta,'descend');
		double[] sorted_theta = Utility.copyArray(theta);
		Arrays.sort(sorted_theta);
		sorted_theta = Utility.reverseArray(sorted_theta);
//		Utility.printArray(sorted_theta);
		double mu = 0.0;
		for (int p=0; p<L; p++) {
			mu = (Utility.sumToP(sorted_theta,p)-pi)/p;
			if ((p < (L-1)) && (sorted_theta[p]>mu) && (sorted_theta[p+1]<=mu)) {
				break;
			}
		}
		double[] newtheta = Utility.maxOrZero(Utility.subtractVectorScalar(theta,mu));
//		Utility.printArray(newtheta);
		return newtheta;
	}
	
	public static double[] project(double[] theta, double pi) {
//		L = length(theta);
//		System.out.println("--------------------------------------");
//		System.out.println("Project input:");
		//		Utility.printArray(theta);
		//		System.out.println("Pi : " + pi);
		int L = theta.length;
		if (theta.length == 0) System.out.println("Theta length is zero!");
//		sorted_theta = sort(theta,'descend');
		double[] sorted_theta = Utility.copyArray(theta);
		Arrays.sort(sorted_theta);
		sorted_theta = Utility.reverseArray(sorted_theta);
//		mus = (cumsum(sorted_theta) - Pi) ./ [1:L]';
		double[] mus = new double[L];
		double[] count = Utility.makeCountVector(L);
		mus = Utility.dividePairwiseVectorVector(Utility.subtractVectorScalar(Utility.cumsum(sorted_theta),pi),count);
//		mu = mus(find(sum(repmat(sorted_theta,1,L) - repmat(mus',L,1) > 0) == [1:L]));
		ArrayList<Double> mu = Utility.getElements(mus,Utility.find(Utility.checkequals(Utility.colsum(Utility.makeGreaterThanZero(Utility.subtractMatrix(Utility.repmatCol(sorted_theta),Utility.repmatRow(mus)))),count)));		
		if (mu.size() != 1) System.out.println("!!!!!! Mu has size != 1, we're in trouble! Mu.size = " + mu.size());
		//if (mu.size() != 1) return theta;
		assert(mu.size() == 1);
		double p = mu.get(0);
//		projected_theta = max(theta - mu , 0);
		double[] newtheta = Utility.maxOrZero(Utility.subtractVectorScalar(theta,p));
		//		System.out.println("--------------------------------------");
		
		return newtheta;
	}
	
		
	private static double[][] getDerivatives(ConflictFunction func, double[][] x) {
	//System.out.println("Input to Derivatives:");
	//	Utility.printMatrix(x);
		double[][] result = func.getDerivativeMatrix(x); 
			/*new double[x.length][x.length];
    	 for (int i = 0; i<x.length; i++) {
    		for (int j = 0; j<x.length; j++) {
//    			System.out.println(func.utility_derivative(k, i, j));
			  result[i][j] = func.utility_derivative(i, i, j); 
    		}
		}*/
    	return result;		
	}
	
	private static double[][] initializeX(double[] pi) {
		double[][] x = Utility.repmatCol(pi);
		x = Utility.divideMatrixConstant(x,pi.length);
		return x;
	}
	
	private static double norm(double[][] in) {
		double sum = 0.0;
		for (int i = 0; i<in.length; i++) {
			for (int j = 0; j<in.length; j++) {
			  sum = sum + in[i][j]*in[i][j];
			}
		}
		return Math.sqrt(sum);
	}
	
/*	private static double error2(double[][] the_new, double[][] the_old, double s) {
		double sum = 0;
		double temp = 0;
		for (int i = 0; i<the_old.length; i++) {
			for (int j = 0; j<the_old.length; j++) {
				temp = the_new[i][j]-the_old[i][j];
				sum = temp*temp; 
			}
		}
		return Math.sqrt(sum);
	}
*/	
	
	private static double error(double[][] the_new, double[][] the_old, double s) {
		double max = 0;
		double temp[][] = new double[the_old.length][the_old.length];
		
		for (int i = 0; i<the_old.length; i++) {
			for (int j = 0; j<the_old.length; j++) {
				temp[i][j] = the_new[i][j]-the_old[i][j];
			}
		}
		double norm=norm(temp); 
//		System.out.println("Norm: " + norm);
		max = norm / s;
//		System.out.println("Max: " + max);
		return max;
	}
	

	
		
}
